/**
  *@file test_imu.c
  *@brief 
  */


#include "bsp_imu.h"
#include "bsp_app.h"
#include "mpu6500_reg.h"
#include "IST8310_reg.h"
#include "spi.h"
#define VAL_MIN(a, b) ((a) < (b) ? (a) : (b))
#define VAL_MAX(a, b) ((a) > (b) ? (a) : (b))

uint8_t MPU_id = 0;
//IMUDataTypedef imu_data = {0,0,0,0,0,0,0,0,0,0};
//IMUDataTypedef imu_data_offest = {0,0,0,0,0,0,0,0,0,0};

static struct mpu_data_info mpu_data;
static struct mpu_calibrate imu_cali = {1, 0, 0};
static uint8_t mpu_buff[14];


static void get_mpu_gyro_offset(void);
static void get_mpu_acc_offset(void);
static void get_ist_mag_offset(void);



//Write a register to MPU6500
uint8_t MPU6500_Write_Reg(uint8_t const reg, uint8_t const data)
{
  static uint8_t MPU_Rx, MPU_Tx;
  
  MPU6500_NSS_Low();
  
  MPU_Tx = reg&0x7f;
  HAL_SPI_TransmitReceive(&hspi5, &MPU_Tx, &MPU_Rx, 1, 55);
  MPU_Tx = data;
  HAL_SPI_TransmitReceive(&hspi5, &MPU_Tx, &MPU_Rx, 1, 55);
  
  MPU6500_NSS_High();
  return 0;
}

//Read a register from MPU6500
uint8_t MPU6500_Read_Reg(uint8_t const reg)
{
  static uint8_t MPU_Rx, MPU_Tx;
  
  MPU6500_NSS_Low();
  
  MPU_Tx = reg|0x80;
  HAL_SPI_TransmitReceive(&hspi5, &MPU_Tx, &MPU_Rx, 1, 55);
  HAL_SPI_TransmitReceive(&hspi5, &MPU_Tx, &MPU_Rx, 1, 55);
  
  MPU6500_NSS_High();
  return MPU_Rx;
}

//Read registers from MPU6500,address begin with regAddr
uint8_t MPU6500_Read_Regs(uint8_t const regAddr, uint8_t *pData, uint8_t len)
{
  static uint8_t MPU_Rx, MPU_Tx, MPU_Tx_buff[14] = {0xff};
  MPU6500_NSS_Low();
  
  MPU_Tx = regAddr|0x80;
  MPU_Tx_buff[0] = MPU_Tx;
  HAL_SPI_TransmitReceive(&hspi5, &MPU_Tx, &MPU_Rx, 1, 55);
  HAL_SPI_TransmitReceive(&hspi5, MPU_Tx_buff, pData, len, 55);
  
  MPU6500_NSS_High();
  return 0;
}

//Write IST8310 register through MPU6500
static void IST_Reg_Write_By_MPU(uint8_t addr, uint8_t data)
{
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_CTRL, 0x00);
  HAL_Delay(2);
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_REG, addr);
  HAL_Delay(2);
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_DO, data);
  HAL_Delay(2);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_CTRL, 0x080 | 0x01);
  HAL_Delay(10);
}

//Write IST8310 register through MPU6500
static uint8_t IST_Reg_Read_By_MPU(uint8_t addr)
{
  uint8_t data;
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV4_REG, addr);
  HAL_Delay(10);
  MPU6500_Write_Reg(MPU6500_I2C_SLV4_CTRL, 0x80);
  HAL_Delay(10);
  data = MPU6500_Read_Reg(MPU6500_I2C_SLV4_DI);
  MPU6500_Write_Reg(MPU6500_I2C_SLV4_CTRL, 0x00);
  HAL_Delay(10);
  return data;
}

//Initialize the MPU6500 I2C Slave0 for I2C reading
static void MPU_Auto_Read_IST_config(uint8_t device_address, uint8_t reg_base_addr, uint8_t data_num)
{
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_ADDR, device_address);
  HAL_Delay(2);
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_REG, IST8310_R_CONFA);
  HAL_Delay(2);
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_DO, IST8310_ODR_MODE);
  HAL_Delay(2);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV0_ADDR, 0x80 | device_address);
  HAL_Delay(2);
  MPU6500_Write_Reg(MPU6500_I2C_SLV0_REG, reg_base_addr);
  HAL_Delay(2);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV4_CTRL, 0x03);
  HAL_Delay(2);
  
  MPU6500_Write_Reg(MPU6500_I2C_MST_DELAY_CTRL, 0x01 | 0x02);
  HAL_Delay(2);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_CTRL, 0x80 | 0x01);
  HAL_Delay(6);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV0_CTRL, 0x80 | data_num);
  HAL_Delay(7);
}

//Initialize the IST8310
uint8_t IST8310_Init(void)
{
  MPU6500_Write_Reg(MPU6500_USER_CTRL, 0x30);
  HAL_Delay(10);
  MPU6500_Write_Reg(MPU6500_I2C_MST_CTRL, 0x0d);
  HAL_Delay(10);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_ADDR, IST8310_ADDRESS);
  HAL_Delay(10);
  MPU6500_Write_Reg(MPU6500_I2C_SLV4_ADDR, 0x80 | IST8310_ADDRESS);
  HAL_Delay(10);
  
  IST_Reg_Write_By_MPU(IST8310_R_CONFB, 0x01);
  if(IST8310_DEVICE_ID_A != IST_Reg_Read_By_MPU(IST8310_WHO_AM_I))
    return 1; //error
  HAL_Delay(10);
  
  IST_Reg_Write_By_MPU(IST8310_R_CONFA, 0x00);
  if(IST_Reg_Read_By_MPU(IST8310_R_CONFA) != 0x00)
    return 2;
  HAL_Delay(10);
  
  IST_Reg_Write_By_MPU(IST8310_R_CONFB, 0x00);
  if(IST_Reg_Read_By_MPU(IST8310_R_CONFB) != 0x00)
    return 3;
  HAL_Delay(10);
  
  IST_Reg_Write_By_MPU(IST8310_AVGCNTL, 0x24);
  if(IST_Reg_Read_By_MPU(IST8310_AVGCNTL) != 0x24)
    return 4;
  HAL_Delay(10);
  
  IST_Reg_Write_By_MPU(IST8310_PDCNTL, 0xc0);
  if(IST_Reg_Read_By_MPU(IST8310_PDCNTL) != 0xc0)
    return 5;
  HAL_Delay(10);
  
  MPU6500_Write_Reg(MPU6500_I2C_SLV1_CTRL, 0x00);
  HAL_Delay(10);
  MPU6500_Write_Reg(MPU6500_I2C_SLV4_CTRL, 0x00);
  HAL_Delay(10);
  
  MPU_Auto_Read_IST_config(IST8310_ADDRESS, IST8310_R_XL, 0x06);
  HAL_Delay(100);
  return 0;
}

//Set the accelerated velocity resolution
uint8_t MPU6500_Set_Accel_Fsr(uint8_t fsr)
{
  return MPU6500_Write_Reg(MPU6500_ACCEL_CONFIG, fsr<<3);
}

//Set the angular velocity resolution
uint8_t MPU6500_Set_Gyro_Fsr(uint8_t fsr)
{
  return MPU6500_Write_Reg(MPU6500_GYRO_CONFIG, fsr<<3);
}


void ist8310_get_data(uint8_t *buff)
{
  MPU6500_Read_Regs(MPU6500_EXT_SENS_DATA_00, buff, 6);
}

//Get 6 axis data from MPU6500
void IMU_Get_Data(struct ahrs_sensor *sensor)
{
  MPU_IO_PROBE();

  MPU6500_Read_Regs(MPU6500_ACCEL_XOUT_H, mpu_buff, 14);

  mpu_data.ax = (mpu_buff[0] << 8 | mpu_buff[1]) - mpu_data.ax_offset;
  mpu_data.ay = (mpu_buff[2] << 8 | mpu_buff[3]) - mpu_data.ay_offset;
  mpu_data.az = (mpu_buff[4] << 8 | mpu_buff[5]) - mpu_data.az_offset;
  mpu_data.temp = mpu_buff[6] << 8 | mpu_buff[7];

  mpu_data.gx = ((mpu_buff[8] << 8 | mpu_buff[9]) - mpu_data.gx_offset);
  mpu_data.gy = ((mpu_buff[10] << 8 | mpu_buff[11]) - mpu_data.gy_offset);
  mpu_data.gz = ((mpu_buff[12] << 8 | mpu_buff[13]) - mpu_data.gz_offset);

  ist8310_get_data((uint8_t *)&mpu_data.mx);

  sensor->ax = mpu_data.ax / (4096.0f / 9.80665f); //8g -> m/s^2
  sensor->ay = mpu_data.ay / (4096.0f / 9.80665f); //8g -> m/s^2
  sensor->az = mpu_data.az / (4096.0f / 9.80665f); //8g -> m/s^2

  sensor->wx = mpu_data.gx / 16.384f / 57.3f; //2000dps -> rad/s
  sensor->wy = mpu_data.gy / 16.384f / 57.3f; //2000dps -> rad/s
  sensor->wz = mpu_data.gz / 16.384f / 57.3f; //2000dps -> rad/s

  sensor->mx = (mpu_data.my - mpu_data.my_offset);
  sensor->my = -(mpu_data.mx - mpu_data.mx_offset);
  sensor->mz = -(mpu_data.mz - mpu_data.mz_offset);

  MPU_IO_PROBE();
}

void mpu_get_temp(float *tmp)
{
  *tmp = 21 + mpu_data.temp / 333.87f;;
}


//Initialize the MPU6500
uint8_t MPU6500_Init(void)
{
  uint8_t index = 0;
    IST_ENABLE();

  MPU6500_Write_Reg(MPU6500_PWR_MGMT_1, 0x80);
  HAL_Delay(100);
  // Reset gyro/accel/temp digital signal path
  MPU6500_Write_Reg(MPU6500_SIGNAL_PATH_RESET, 0x07);
  HAL_Delay(100);
  if(MPU6500_ID != MPU6500_Read_Reg(MPU6500_WHO_AM_I))  //read id of device,check if MPU6500 or not
  {
      return 1;
  }
  HAL_Delay(100);
  uint8_t MPU6500_Init_Data[7][2] = 
  {
//    {MPU6500_PWR_MGMT_1,    0x80},      // Reset Device
    {MPU6500_PWR_MGMT_1,    0x03},      // Clock Source - Gyro-Z
    {MPU6500_PWR_MGMT_2,    0x00},      // Enable Acc & Gyro
    {MPU6500_CONFIG,        0x02},      // LPF 98Hz
    {MPU6500_GYRO_CONFIG,   0x18},      // +-2000dps
    {MPU6500_ACCEL_CONFIG,  0x10},      // +-8G
    {MPU6500_ACCEL_CONFIG_2,0x02},      // enable LowPassFilter  Set Acc LPF
    {MPU6500_USER_CTRL,     0x20},      // Enable AUX
  };
  
 
  
  for(index = 0; index < 7; index++)
  {
    MPU6500_Write_Reg(MPU6500_Init_Data[index][0], MPU6500_Init_Data[index][1]);
    HAL_Delay(1);
  }
  
   IST8310_Init();
   if(imu_cali.gyro_flag == 1)
  {
    get_mpu_gyro_offset();
  }

  if (imu_cali.acc_flag == 1)
  {
    get_mpu_acc_offset();
  }

  if (imu_cali.mag_flag == 1)
  {
    get_ist_mag_offset();
  }

  return 0;
}

static void get_mpu_gyro_offset(void)
{
  int i;
  for (i = 0; i < 300; i++)
  {
    MPU6500_Read_Regs(MPU6500_ACCEL_XOUT_H, mpu_buff, 14);

    mpu_data.gx_offset += mpu_buff[8] << 8 | mpu_buff[9];
    mpu_data.gy_offset += mpu_buff[10] << 8 | mpu_buff[11];
    mpu_data.gz_offset += mpu_buff[12] << 8 | mpu_buff[13];

    MPU_DELAY(2);
  }

  mpu_data.gx_offset = mpu_data.gx_offset / 300;
  mpu_data.gy_offset = mpu_data.gy_offset / 300;
  mpu_data.gz_offset = mpu_data.gz_offset / 300;
  imu_cali.gyro_flag = 0;
}

static void get_mpu_acc_offset(void)
{
  int i;
  for (i = 0; i < 300; i++)
  {
    MPU6500_Read_Regs(MPU6500_ACCEL_XOUT_H, mpu_buff, 14);

    mpu_data.ax_offset += mpu_buff[0] << 8 | mpu_buff[1];
    mpu_data.ay_offset += mpu_buff[2] << 8 | mpu_buff[3];
    mpu_data.az_offset += mpu_buff[4] << 8 | mpu_buff[5] - 4096;

    MPU_DELAY(2);
  }

  mpu_data.ax_offset = mpu_data.ax_offset / 300;
  mpu_data.ay_offset = mpu_data.ay_offset / 300;
  mpu_data.az_offset = mpu_data.az_offset / 300;

  imu_cali.acc_flag = 0;
}

static void get_ist_mag_offset(void)
{
  int16_t mag_max[3], mag_min[3];
  int i;
  for (i = 0; i < 5000; i++)
  {
    ist8310_get_data((uint8_t *)&mpu_data.mx);
    if ((abs(mpu_data.mx) < 400) && (abs(mpu_data.my) < 400) && (abs(mpu_data.mz) < 400))
    {
      mag_max[0] = VAL_MAX(mag_max[0], mpu_data.mx);
      mag_min[0] = VAL_MIN(mag_min[0], mpu_data.mx);

      mag_max[1] = VAL_MAX(mag_max[1], mpu_data.my);
      mag_min[1] = VAL_MIN(mag_min[1], mpu_data.my);

      mag_max[2] = VAL_MAX(mag_max[2], mpu_data.mz);
      mag_min[2] = VAL_MIN(mag_min[2], mpu_data.mz);
    }
    MPU_DELAY(2);
  }
  mpu_data.mx_offset = (int16_t)((mag_max[0] + mag_min[0]) * 0.5f);
  mpu_data.my_offset = (int16_t)((mag_max[1] + mag_min[1]) * 0.5f);
  mpu_data.mz_offset = (int16_t)((mag_max[2] + mag_min[2]) * 0.5f);

  imu_cali.mag_flag = 0;
}



